/*
 * TODO: Description
 *
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "OfmsSimpleCar.h"
#include <cassert>

#include <OgreBulletDynamicsRigidBody.h>
//#include <OgreBulletDynamicsWorld.h>
//#include <OgreBulletCollisionsWorld.h>


// todo: when this filehas taken over the functions of he Tick Listener, remove that file(?)


using namespace OpenFms;

/** 
 * Vehicle class constructor
 */
OfmsSimpleCar::OfmsSimpleCar() :
	m_pSceneNode(NULL),
	m_pInputDevice(NULL),
	m_pEntity(NULL),
	m_pCollisionShape(NULL),
	m_pRigidBody(NULL),
	m_pWheelShape(NULL),
	m_pFrontLeftWheel(NULL),
	m_pFrontRightWheel(NULL),
	m_pRearLeftWheel(NULL),
	m_pRearRightWheel(NULL),
	m_pFrontLeftWheelConstraint(NULL),
	m_pFrontRightWheelConstraint(NULL),
	m_pRearLeftWheelConstraint(NULL),
	m_pRearRightWheelConstraint(NULL)
{
	memset( &m_inertias, 0, sizeof m_inertias );
	m_inertias.size = sizeof m_inertias;

	memset( &m_environment, 0, sizeof(ModelSimulator_environment_t));
	m_environment.size = sizeof(ModelSimulator_environment_t);
}

/** 
 * Vehicle class destructor
 */
OfmsSimpleCar::~OfmsSimpleCar() 
{
	unload();
	BulletTickListener::~BulletTickListener();
}


size_t OfmsSimpleCar::m_uniqueCount = 0;
Ogre::String OfmsSimpleCar::generateUniqueName()
{
	char buffer[10];
	itoa( ++m_uniqueCount , buffer, 10 );
	return Ogre::String( "OfmsSimpleCar_") + Ogre::String(buffer) ;
}


/**
 * Load a vehicle from a ogre resource group
 *
 * Creates visual and physical models, and loads a simulator.
 *
 * @return Returns true on success, false on fail.
 */
// todo error messages
bool OfmsSimpleCar::load( const Ogre::String &group, OgreBulletDynamics::DynamicsWorld *pWorld, Ogre::SceneManager *pSceneManager  )
{
	// first unload anything that is already loaded
	unload();
	assert( m_pEntity == NULL && "There should not be an entity (yet)" );
	assert( m_pCollisionShape == NULL && "There should not be a collision shape (yet)");
	assert( m_pRigidBody == NULL && "There should be no rigid body (yet)");
	assert( m_pSceneNode == NULL && "There should be no scene node (yet)");

	// Set Inertias
	m_inertias.size = sizeof m_inertias; // just in case getInertias changes this.
	m_inertias.mass = 2.f;
	m_inertias.ixx = 2.f;
	m_inertias.iyy = 2.f;
	m_inertias.izz = 2.f;
	m_inertias.ixy = 0.f;
	m_inertias.ixz = 0.f;
	m_inertias.iyz = 0.f;

	// Create Ogre Entities
	m_pEntity = pSceneManager->createEntity(generateUniqueName(), "carbody.mesh", group );
	Ogre::Entity *pFrontRightEntity = pSceneManager->createEntity( generateUniqueName(), "carwheel.mesh", group );
	Ogre::Entity *pFrontLeftEntity  = pSceneManager->createEntity( generateUniqueName(), "carwheel.mesh", group );
	Ogre::Entity *pRearRightEntity  = pSceneManager->createEntity( generateUniqueName(), "carwheel.mesh", group );
	Ogre::Entity *pRearLeftEntity   = pSceneManager->createEntity( generateUniqueName(), "carwheel.mesh", group );

	// Set materials
	//m_pEntity->setMaterialName( "carbodymaterial", group );
	//pFrontRightEntity->setMaterialName( "carwheelmaterial", group );
	//pFrontLeftEntity->setMaterialName( "carwheelmaterial", group );
	//pRearRightEntity->setMaterialName( "carwheelmaterial", group );
	//pRearLeftEntity->setMaterialName( "carwheelmaterial", group );

	// Attach to visual World
	m_pSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode( generateUniqueName() );
	m_pSceneNode->attachObject( m_pEntity );
	Ogre::SceneNode *pFrontLeftSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode(generateUniqueName());
	pFrontLeftSceneNode->attachObject(pFrontRightEntity);
	Ogre::SceneNode *pFrontRightSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode(generateUniqueName());
	pFrontRightSceneNode->attachObject(pFrontLeftEntity);
	Ogre::SceneNode *pRearLeftSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode(generateUniqueName());
	pRearLeftSceneNode->attachObject(pRearRightEntity);
	Ogre::SceneNode *pRearRightSceneNode = pSceneManager->getRootSceneNode()->createChildSceneNode(generateUniqueName());
	pRearRightSceneNode->attachObject(pRearLeftEntity);

	// Create physical body shape
	Ogre::Vector3 bodySize = (m_pEntity->getBoundingBox()).getHalfSize();
	m_pCollisionShape = new OgreBulletCollisions::BoxCollisionShape( bodySize );

	// Create physical wheel shape
	// note: rotation axis is Z, This semms to be the deafult in bullet, 
	// and can be worked around, but easier just to go with the flow.
	Ogre::Vector3 wheelSize =(pFrontRightEntity->getBoundingBox()).getHalfSize();
	m_pWheelShape = new OgreBulletCollisions::CylinderCollisionShape(wheelSize, Ogre::Vector3(0,0,1) );

	// Instanciate physical body
	const float mass = m_inertias.mass;
	const Ogre::Vector3 position(0,0, -(wheelSize.x+bodySize.z+.1f));
	const Ogre::Quaternion orientation(0,0,0,1);
	const float restitution = .1f;
	const float friction = 1.5f;
	m_pRigidBody = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	m_pRigidBody->setShape( m_pSceneNode, m_pCollisionShape, restitution, friction, mass, position, orientation );
	m_pRigidBody->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!


	// Instanciate front left wheel
	const Ogre::Vector3 frontLeftPosition = Ogre::Vector3( bodySize.x-wheelSize.x-wheelSize.x, -(bodySize.y+wheelSize.z),wheelSize.x);
	const Ogre::Quaternion frontLeftOrientation( Ogre::Radian(SIMD_PI/2), Ogre::Vector3(1,0,0));
	m_pFrontLeftWheel = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	m_pFrontLeftWheel->setShape( pFrontLeftSceneNode, m_pWheelShape, restitution, friction, .1f, frontLeftPosition, frontLeftOrientation );
	m_pFrontLeftWheel->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!

	// Instantiate front right wheel
	const Ogre::Vector3 frontRightPosition = Ogre::Vector3(bodySize.x-wheelSize.x-wheelSize.x,(bodySize.y+wheelSize.z),wheelSize.x);
	const Ogre::Quaternion frontRightOrientation( Ogre::Radian(-SIMD_PI/2), Ogre::Vector3(1,0,0));
	m_pFrontRightWheel = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	m_pFrontRightWheel->setShape( pFrontRightSceneNode, m_pWheelShape, restitution, friction, .1f, frontRightPosition, frontRightOrientation );
	m_pFrontRightWheel->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!

	// Instanciate front left wheel
	const Ogre::Vector3 rearLeftPosition = Ogre::Vector3(-bodySize.x+wheelSize.x+wheelSize.x,-(bodySize.y+wheelSize.z),wheelSize.x);
	const Ogre::Quaternion rearLeftOrientation( frontLeftOrientation );
	m_pRearLeftWheel = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	m_pRearLeftWheel->setShape( pRearLeftSceneNode, m_pWheelShape, restitution, friction, .1f, rearLeftPosition, rearLeftOrientation );
	m_pRearLeftWheel->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!

	// Instanciate front right wheel
	const Ogre::Vector3 rearRightPosition = Ogre::Vector3(-bodySize.x+wheelSize.x+wheelSize.x,(bodySize.y+wheelSize.z),wheelSize.x);
	const Ogre::Quaternion rearRightOrientation( frontRightOrientation );
	m_pRearRightWheel = new OgreBulletDynamics::RigidBody( generateUniqueName(), pWorld );
	m_pRearRightWheel->setShape( pRearRightSceneNode, m_pWheelShape, restitution, friction, .1f, rearRightPosition, rearRightOrientation );
	m_pRearRightWheel->getBulletRigidBody()->setActivationState(DISABLE_DEACTIVATION); // !!!
	
	// setup rear left wheel constraint ( a hinge constraint )
	const btTransform rearLeftA(btQuaternion(0,0,0,1), btVector3(0,0,0));
	const btTransform rearLeftB( OgreBulletCollisions::OgreBtConverter::to( rearLeftOrientation ), OgreBulletCollisions::OgreBtConverter::to( rearLeftPosition ));
	m_pRearLeftWheelConstraint = new btHingeConstraint( 
		*(m_pRearLeftWheel->getBulletRigidBody()),
		*(m_pRigidBody->getBulletRigidBody()), 
		rearLeftA, rearLeftB, true );
	pWorld->getBulletDynamicsWorld()->addConstraint( m_pRearLeftWheelConstraint, true );

	// setup rear right wheel constraint ( a hinge constraint )
	const btTransform rearRightA(btQuaternion(0,0,0,1), btVector3(0,0,0));
	const btTransform rearRightB( OgreBulletCollisions::OgreBtConverter::to( rearRightOrientation ), OgreBulletCollisions::OgreBtConverter::to( rearRightPosition ));
	m_pRearRightWheelConstraint = new btHingeConstraint( 
		*(m_pRearRightWheel->getBulletRigidBody()),
		*(m_pRigidBody->getBulletRigidBody()), 
		rearRightA, rearRightB, true );
	pWorld->getBulletDynamicsWorld()->addConstraint( m_pRearRightWheelConstraint, true );

	// setup rear left wheel constraint ( 6-dof constraint )
	const btTransform frontLeftA(btQuaternion(0,0,0,1), btVector3(0,0,0));
	const btTransform frontLeftB( OgreBulletCollisions::OgreBtConverter::to( frontLeftOrientation ),OgreBulletCollisions::OgreBtConverter::to( frontLeftPosition ));
	m_pFrontLeftWheelConstraint = new btGeneric6DofConstraint(
		*(m_pFrontLeftWheel->getBulletRigidBody()),
		*(m_pRigidBody->getBulletRigidBody()), 
		frontLeftA, frontLeftB, true );
	m_pFrontLeftWheelConstraint->setAngularLowerLimit(btVector3(0,0,1));
	m_pFrontLeftWheelConstraint->setAngularUpperLimit(btVector3(0,0,-1));
	pWorld->getBulletDynamicsWorld()->addConstraint( m_pFrontLeftWheelConstraint, true );

	// setup rear left wheel constraint ( 6-dof constraint )
	const btTransform frontRightA(btQuaternion(0,0,0,1), btVector3(0,0,0));
	const btTransform frontRightB( OgreBulletCollisions::OgreBtConverter::to( frontRightOrientation ),OgreBulletCollisions::OgreBtConverter::to( frontRightPosition ));
	m_pFrontRightWheelConstraint = new btGeneric6DofConstraint(
		*(m_pFrontRightWheel->getBulletRigidBody()),
		*(m_pRigidBody->getBulletRigidBody()), 
		frontRightA, frontRightB, true );
	m_pFrontRightWheelConstraint->setAngularLowerLimit(btVector3(0,0,1));
	m_pFrontRightWheelConstraint->setAngularUpperLimit(btVector3(0,0,-1));
	pWorld->getBulletDynamicsWorld()->addConstraint( m_pFrontRightWheelConstraint, true );
	
	return false;
}

/**
 * Callback function that reads its data from a string
 *
 * Its a function of the ModelSimulator_read_callback_t type. takes the address of a pointer containing the 
 * current position in the string.
 */
static uint64_t __cdecl stringreader( uint64_t _custom, uint64_t _buffer, uint64_t _buffer_size ) // num_bytes_read ( custom, buffer_start, buffer_size )
{
    assert( _custom && _buffer && _buffer_size );

    const char **pp = reinterpret_cast<const char **>(_custom);
    char *p = reinterpret_cast<char *>(_buffer);
    size_t p_length = static_cast<size_t>(_buffer_size);
    size_t pp_length = strlen(*pp);

    if( pp_length <= p_length ) {
        memcpy(p, *pp, pp_length);
        *pp += pp_length;
        return static_cast<uint64_t>(pp_length);
    }
    else {
        memcpy(p, *pp, p_length);
        *pp += p_length;
        return static_cast<uint64_t>(p_length);
    }
}

/**
 * Frees loaded resources
 *
 * First detaches visual and physical
 */

#define DELETE_AND_NULL(X) if(X) { delete X; X=NULL; }
void OfmsSimpleCar::unload()
{
	DELETE_AND_NULL(m_pWheelShape);

	DELETE_AND_NULL(m_pFrontLeftWheel);
	DELETE_AND_NULL(m_pFrontRightWheel);
	DELETE_AND_NULL(m_pRearLeftWheel);
	DELETE_AND_NULL(m_pRearRightWheel);
	
	DELETE_AND_NULL(m_pFrontLeftWheelConstraint);
	DELETE_AND_NULL(m_pFrontRightWheelConstraint);
	DELETE_AND_NULL(m_pRearLeftWheelConstraint);
	DELETE_AND_NULL(m_pRearRightWheelConstraint);

	DELETE_AND_NULL(m_pCollisionShape);
	DELETE_AND_NULL(m_pWheelShape);

	// destroy rigid body before deleting the ogre stuff becouse 
	// is contains reference to the visual object?
	DELETE_AND_NULL(m_pRigidBody);

	// detach from visual world
	if( m_pSceneNode )
	{
		//todo: the following deletes all shicl nodes, but I dont think it
		// removes the node itself (this causes no memory leaks, but just takes
		// up a litte space untill the whole program unloads
		m_pSceneNode->removeAndDestroyAllChildren();
		m_pSceneNode=NULL;
	}

	/// todo: if i run the following, there will be an access violation when the ogre root 
	// is deleted, find out why, and "fix" it. (Again, this causes no memory leaks, but it
	// takes up unnessecary space untill the whole program exits)
	/*if( m_pEntity ) {
		delete m_pEntity;
		m_pEntity=NULL;
	}*/


}



/**
 * Attach an Input Device
 */
void OfmsSimpleCar::attachInput( InputDevice *pInput )
{
	m_pInputDevice = pInput;
}

/**
 * Detach an Input Device
 */
void OfmsSimpleCar::detachInput()
{
	m_pInputDevice = NULL;
}

/**
 * Capture data from the attached Input Device
 */
void OfmsSimpleCar::captureInput()
{
	if( m_pInputDevice ) {
		m_pInputDevice->capture( m_environment.channel, 0, m_nChannels );
	}
}

Ogre::SceneNode *OfmsSimpleCar::getSceneNode() const
{
	return m_pSceneNode;
}

/**
 * This function is called once before each frame is rendered, and before the physics simulator has stepped
 */
void OfmsSimpleCar::onFrameStarted(const Ogre::FrameEvent &evt)
{
	// Capture the input here, this capture is done once for each frame
	captureInput();
	const float angle = m_environment.channel[3];
	if( m_pFrontLeftWheelConstraint ) {
		m_pFrontLeftWheelConstraint->setAngularLowerLimit(btVector3(0,angle,1));
		m_pFrontLeftWheelConstraint->setAngularUpperLimit(btVector3(0,angle,-1));
	}
	if( m_pFrontRightWheelConstraint ) { 
		m_pFrontRightWheelConstraint->setAngularLowerLimit(btVector3(0,-angle,1));
		m_pFrontRightWheelConstraint->setAngularUpperLimit(btVector3(0,-angle,-1));
	}
	
}

/** 
 * Convert captured input and to forces, and apply them
 *
 * Note: the inputs are captured in onFrameStarted, and not here
 * this way they only gets capturen once for each frame, 
 * since this function can be calles multiple times between frames.
 */
void OfmsSimpleCar::onTick(const btDynamicsWorld *world, btScalar timeStep)
{
	// Apply forces given the cpatured input.
	// todo: check if sim is loaded:
	if(m_pRigidBody /*&& m_pModelSimulator*/) {
		btRigidBody *pBulletRigidBody;

		// Apply gravity on main body
		pBulletRigidBody = m_pRigidBody->getBulletRigidBody();
		assert( pBulletRigidBody != NULL && "Must not be null");
		pBulletRigidBody->clearForces();
		pBulletRigidBody->applyGravity();

		// get the orientation of the airplane
		btQuaternion orientation = pBulletRigidBody->getOrientation();
		btQuaternion orientationInv = orientation.inverse();

		//const float throttle = -20*m_environment.channel[2];
		const float throttle = 2*m_environment.channel[2];
		const btVector3 torqueLeft(0,0,throttle);
		const btVector3 torqueRight(0,0,-throttle);
		//pBulletRigidBody->applyForce( quatRotate( orientation, force ), btVector3(0,0,0) );

		if(m_pRearLeftWheel) {
			pBulletRigidBody = m_pRearLeftWheel->getBulletRigidBody();
			if(pBulletRigidBody) { 
				pBulletRigidBody->applyTorque( quatRotate( pBulletRigidBody->getOrientation(), torqueLeft) );
			}
		}
		if(m_pRearRightWheel) {
			pBulletRigidBody = m_pRearRightWheel->getBulletRigidBody();
			if(pBulletRigidBody) { 
				pBulletRigidBody->applyTorque( quatRotate( pBulletRigidBody->getOrientation(), torqueRight) );
			}
		}

		if(m_pFrontLeftWheel) {
			pBulletRigidBody = m_pFrontLeftWheel->getBulletRigidBody();
			if(pBulletRigidBody) { 
				pBulletRigidBody->applyTorque( quatRotate( pBulletRigidBody->getOrientation(), torqueLeft) );
			}
		}
		if(m_pFrontRightWheel) {
			pBulletRigidBody = m_pFrontRightWheel->getBulletRigidBody();
			if(pBulletRigidBody) { 
				pBulletRigidBody->applyTorque( quatRotate( pBulletRigidBody->getOrientation(), torqueRight) );
			}
		}

		// note: the above is just (-orientation), but there seems to be
		// an ambiguity with the negate operator.

		// Set up the apparent windspeed
		// note: the rest of m_environment is set up elsewhere
		//btVector3 v;
		//v = quatRotate(orientationInv, pBulletRigidBody->getLinearVelocity());
		//m_environment.linearVelocityX = static_cast<float>(v.getX());
		//m_environment.linearVelocityY = static_cast<float>(v.getY());
		//m_environment.linearVelocityZ = static_cast<float>(v.getZ());
		//v = quatRotate(orientationInv, pBulletRigidBody->getAngularVelocity());
		//m_environment.angularVelocityX = static_cast<float>(v.getX());
		//m_environment.angularVelocityY = static_cast<float>(v.getY());
		//m_environment.angularVelocityZ = static_cast<float>(v.getZ());

		// call the model simulator to get the forces, given the environment
		// todo: move the setStepSize somwhere else
		//ModelSimulator_forces_t forces = { sizeof(ModelSimulator_forces_t) };
		//m_simulator.setStepSize(static_cast<float>(timeStep));
		//m_simulator.step( m_environment, forces );
		
		// apply the forces to the object,
		// but first, convert the forces to world coodrinates
		//btVector3 force(forces.force_x, forces.force_y, forces.force_z);
		//btVector3 torque(forces.torque_x, forces.torque_y, forces.torque_z);
		//pBulletRigidBody->applyForce(quatRotate(orientation, force), btVector3(0,0,0));
		//pBulletRigidBody->applyTorque(quatRotate(orientation, torque));
	}
}

